//
// Created by wangdayong on 20-12-7.
//
#include "IMUReader.h"
#include <stdio.h>
#include <cstring>
#include "gyro2quat.h"

FILE * fd;

int node_open()
{
    if(fd == nullptr){
//        fd = fopen("/dev/hidraw0","r");
        fd = fopen("/sdcard/Download/IMU_rawdata.bin","r");
        if(fd == nullptr){
            PRINT("open node failed");
            return -1;
        }
    }
    return 0;
}

int node_close()
{
    if(fd != nullptr){
        return fclose(fd);
    }else{
        return -123;
    }
}

NodeData node_read()
{
    char buffer[15] = {0};
    NodeData data;
    short int t;
    if(fd != nullptr) {
        int r = fread(buffer, 15, 1, fd);
//        PRINT("fread return %d, %s",r,buffer);

        memcpy(&t, buffer + 0, 2);
        data.acc_x = ( t * 9.8f ) / (0x8000 / 2);

        memcpy(&t, buffer + 2, 2);
        data.acc_y = ( t * 9.8f ) / (0x8000 / 2);

        memcpy(&t, buffer + 4, 2);
        data.acc_z = ( t * 9.8f ) / (0x8000 / 2);

        short int m = 0x7fff;
        float range = 2000.f * 3.1415926f / 180.f;

        memcpy(&t, buffer + 6, 2);
        data.gyro_x = ( t * range ) / m;

        memcpy(&t, buffer + 8, 2);
        data.gyro_y = ( t * range ) / m;

        memcpy(&t, buffer + 10, 2);
        data.gyro_z = ( t * range ) / m;

        memcpy(&data.timeStamp, buffer+12, 3);
        data.timeStamp >> 4;
        data.timeStamp = data.timeStamp & 0x0fffffff;

        PRINT("node_read | gyro %f, %f, %f | acc %f, %f, %f | time %d",
              data.gyro_x, data.gyro_y, data.gyro_z,
              data.acc_x, data.acc_y, data.acc_z,
              data.timeStamp);
    }
    return data;
}